/*
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
*/
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <SCServo.h>

SMS_STS sms_sts;

int LEDpin = 2;
void setup()
{
	pinMode(LEDpin, OUTPUT);
	digitalWrite(LEDpin, HIGH);
	Serial.begin(115200);
	// Serial1.begin(115200); // sms舵机波特率115200
	Serial2.begin(1000000); // sts舵机波特率1000000
	sms_sts.pSerial = &Serial2;
	delay(1000);
}

void loop()
{
	int ID = sms_sts.Ping(1);
	if (ID != -1)
	{
		digitalWrite(LEDpin, LOW);
		Serial.print("Servo ID:");
		Serial.println(ID, DEC);
		delay(100);
	}
	else
	{
		Serial.println("Ping servo ID error!");
		digitalWrite(LEDpin, HIGH);
		delay(2000);
	}
}
